$TITLE ('RTX-51 QUEUE OPERATIONS')
$SYMBOLS
$NOXREF
$NOCOND
$NOMOD51
$NORB
$PAGELENGTH(80) PAGEWIDTH(110)
;************************************************************************
;*									*
;*    R T X - 5 1  :   QUEUE OPERATIONS                                 *
;*							     		*
;*----------------------------------------------------------------------*
;*                                                                      *
;*    Filename     :   RTXQUOP.A51                                      *
;*    Language     :   Keil A-51                                        *
;*    Dev. system  :   Keil uVision2                                    *
;*    Targetsystem :   Any system based upon 8051 up                    *
;*                                                                      *
;*    Purpose      :   Functions for enqueueing and dequeueing of RTX   *
;*                     tasks. Handles the following queues:             *
;*                                                                      *
;*                              - interrupts wait list                  *
;*                              - signal wait list                      *
;*                              - timeout wait list                     *
;*                              - message wait lists (empty msg list)   *
;*                              - non full wait lists (full msg list)   *
;*                              - semaphore wait lists                  *
;*								     	*
;*----------------------------------------------------------------------*
;* Rev. |  Released   | Programmer    | Comments                        *
;*----------------------------------------------------------------------*
;*  0.1 |  24.1.90    | E. Glatz      | File created                    *
;*  0.2 |   8.3.91    | Th. Fischler  | Changes for V4.0                *
;*      |   6.5.91    |               | More changes                    *
;*      |   9.7.91    |               | Global interrupt disable while  *
;*      |             |               | mailbox access                  *
;*      |  28.8.91    |               | Timeout values 0..254 are       *
;*      |             |               | allowed                         *
;*      |  23.10.91   | Th. Fischler  | Little corrections              *
;*      |  12.12.91   |               | ?RTX_MSG_ENQUE, enable INT's    *
;*      |             |               | after all Mailbox-variables are *
;*      |             |               | updated --> shorter interrupt   *
;*      |             |               | latency                         *
;*      | 25.6.92     |               | Global interrupt disable while  *
;*      |             |               | updating the interrupt mask-    *
;*      |             |               | variables -> for the set/reset  *
;*      |             |               | int_mask functions              *
;*      | 21.10.92    | ThF           | The legendary Timeout error     *
;*      |             |               | corrected (overflow check)      *
;* 5.0  | 10.10.1994  | E. Glatz      | Inhibit enq if !?RTX_MBX_SUPPORT*
;* 4.25 | 11.10.94    |               | NFL_DEQUEUE: FIFO wrap corrected*
;* 5.0  | 11.10.1994  |               | ?RTX_SEM_ENQUEUE/DEQUEUE        *
;* 7.00 | 13.11.2001  | T. Mueller    | Release 7.00                    *
;************************************************************************
;*    Copyright 1991 .. 2001 METTLER  &  FUCHS  AG,  CH-8953 Dietikon   *
;************************************************************************
																			
$INCLUDE (RTXIMASK.INC)

;*----------------------------------------------------------------------*
;*	I M P O R T S
;*----------------------------------------------------------------------*

$INCLUDE (RTXCONST.INC)
$INCLUDE (RTXEXTRN.INC)

;*----------------------------------------------------------------------*
;*	E X P O R T S
;*----------------------------------------------------------------------*

PUBLIC   ?RTX_SIG_ENQUEUE, ?RTX_SIG_DEQUEUE, ?RTX_MSG_ENQUEUE
PUBLIC   ?RTX_MSG_DEQUEUE, ?RTX_NFL_ENQUEUE, ?RTX_NFL_DEQUEUE
PUBLIC   ?RTX_TMO_ENQUEUE, ?RTX_TMO_DEQUEUE, ?RTX_IVL_ENQUEUE
PUBLIC   ?RTX_INT_ENQUEUE
PUBLIC   ?RTX_INTSIG_DEQUEUE, ?RTX_SEM_ENQUEUE, ?RTX_SEM_DEQUEUE

;*----------------------------------------------------------------------*
;*	P R O C E D U R E S
;*----------------------------------------------------------------------*

;*----------------------------------------------------------------------*
;*      ? R T X _ S I G _ E N Q U E U E
;*----------------------------------------------------------------------*
;*      Enqueue task in signal wait list. Set wait bit in task state.
;*----------------------------------------------------------------------*
;*      Input:   - DPTR points to task state of task to be enqueued
;*               - R4  = 0: conditional enqueue (do not enqueue, if
;*                          no signal is pending)
;*                    <> 0: unconditional enqueue (enqueue, if no
;*                          signal is pending)
;*
;*      Output:  - R4 and DPTR preserved
;*               - ACC changed
;*               - Event status returned in ACC: = 1: signal received
;*                                               = 0: no signal pending
;*----------------------------------------------------------------------*

?RTX?SIG_ENQUEUE?RTXQUOP  SEGMENT CODE
                          RSEG    ?RTX?SIG_ENQUEUE?RTXQUOP

?RTX_SIG_ENQUEUE:
               ; Get task state
               MOVX  A, @DPTR
               ; Check if signal pending
               JB    ACC.7, SIG_PENDING

               ; Check if conditional enq.
               CJNE  R4, #0, SIG_ENQ
               ; Return 'no signal pending'
               CLR   A
               RET

SIG_ENQ:       ; Set signal wait bit
               ORL   A, #004H                   ; in task state
               MOVX  @DPTR, A
               ; Return 'no signal pending'
               CLR   A
               RET

SIG_PENDING:   ; Clear SR bit in task state
               ANL   A, #07FH
               MOVX  @DPTR, A
               ; Return 'signal pending'
               MOV   A, #1
               RET


;*----------------------------------------------------------------------*
;*      ? R T X _ S I G _ D E Q U E U E
;*----------------------------------------------------------------------*
;*      Dequeue task from signal wait list. Reset wait bit in task state.
;*----------------------------------------------------------------------*
;*      Input:   - DPTR points to task state of task to be dequeued
;*
;*      Output:  - DPTR preserved
;*               - ACC changed
;*----------------------------------------------------------------------*

?RTX?SIG_DEQUEUE?RTXQUOP  SEGMENT CODE
                          RSEG    ?RTX?SIG_DEQUEUE?RTXQUOP

?RTX_SIG_DEQUEUE:
               ; Get task state
               MOVX  A, @DPTR
               ; Clear sig wait bit in task state
               ANL   A, #0FBH
               MOVX  @DPTR, A
               RET



;*----------------------------------------------------------------------*
;*      ? R T X _ M S G _ E N Q U E U E
;*----------------------------------------------------------------------*
;*      Enqueue task if no messages are available.
;*      Task is not enqueued, if:
;*              a) a message is available
;*              b) no message is available, but a conditional enqueue
;*                 was specified
;*              c) task list is full (already 16 waiting tasks)
;*      If task is enqueued, then the wait bit in the task state is set
;*      and the mailbox number is stored, as well.
;*----------------------------------------------------------------------*
;*      Input:   - ACC: Bits 0,1,2 contain mailbox number
;*               - DPTR points to task state of task to be enqueued
;*               - ?RTX_RUNNING_TID contains task id of task to be enqueued
;*               - R4  = 0: conditional enqueue (do not enqueue, if
;*                          no message is available)
;*                    <> 0: unconditional enqueue (enqueue, if no
;*                          message is available)
;*               - (R2:R3) points to message return variable
;*
;*      Output:  - ?RTX_RUNNING_TID, R4, R2, R3 preserved
;*               - Event status returned in ACC: = 0: message available
;*                                               = 1: no message available
;*                                               = 2: no msg, but wait
;*                                                    list full (not queued)
;*               - If ACC=0: DPTR points to MBX#_COUNT, all interrupts
;*                 are disabled (GLOBAL_INT_DISABLE)
;*               - ACC, B, R0, R1, R5, R6 and DPTR changed
;*----------------------------------------------------------------------*

?RTX?MSG_ENQUEUE?RTXQUOP  SEGMENT CODE
                          RSEG    ?RTX?MSG_ENQUEUE?RTXQUOP

?RTX_MSG_ENQUEUE:
               ; Check if messages available, Get MBX#_COUNT
               MOV   R0, DPH
               MOV   R1, DPL                    ; Store DPTR
               ANL   A, #07H                    ; Extract mailbox number
               SWAP  A
               MOV   B, A                       ; Store mbx no for later
               RR    A                          ; from task state, shift
               ADD   A, #LOW(?RTX_MBX_CONTROL_BASE) ; Add base of mbx-cont-block
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_MBX_CONTROL_BASE) ; DPTR --> MBX#_COUNT
               GLOBAL_INT_DISABLE
               MOVX  A, @DPTR

               ; Check if messages available
               DEC   A
               JNB   ACC.7, MSGS_WAIT
               ; Check if conditional wait
               CJNE  R4, #0, MSG_ENQ
               ; Set return 'no msg available'
               MOV   A, #1
               GLOBAL_INT_ENABLE
               RET

MSGS_WAIT:     ; At least one message is available
               CLR   A                          ; Set return 'msg available'
               RET

MSG_ENQ:       ; Enqueue task in message wait list
               ; Check if message list size <> 0
               JNB   ?RTX_MBX_SUPPORT, MFULL
               ; Check if wait list not full
               CJNE  A, #0EFH, NOT_FULL
MFULL:         ; Set return 'no msg,list full'
               MOV   A, #2
               GLOBAL_INT_ENABLE
               RET

NOT_FULL:      ; Store new MBX#_COUNT
               MOVX  @DPTR, A
               ; Set wait flag and store mailbox no
               SETB  B.0
               ; Swap DPTR and R0:R1
               MOV   A, DPH
               XCH   A, R0
               MOV   DPH, A
               MOV   A, DPL
               XCH   A, R1
               MOV   DPL, A

               MOVX  A, @DPTR
               ANL   A, #08FH
               ORL   A, B
               MOVX  @DPTR, A

               ; Get, increment and store MBX#_LAST
               MOV   DPH, R0
               MOV   DPL, R1                    ; Get ptr. to mailbox_control
               INC   DPL
               INC   DPL
               INC   DPL
               MOVX  A, @DPTR                   ; get MBX#_LAST
               INC   A
               ANL   A, #0EFH
               MOVX  @DPTR, A

               ; Store task at end of FIFO
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_MBX_PAGE)
               MOV   A, ?RTX_RUNNING_TID
               MOVX  @DPTR, A
               ; Mailbox var's updated, reenable ISR'Ints
               GLOBAL_INT_ENABLE

               ; Store address of message return var. in task context
               JB   ?RTX_RUNNING_TID.4, DRIVERS
               ; Standard tasks (=non driver tasks) get address of 
               ; context-area
NON_D:         MOV   B, ?RTX_RUNNING_TID
               ANL   B, #00FH                   ; Get standard task number
               MOV   DPH, #HIGH(?RTX_KNTXT_PTR_TABLE)
               MOV   A, #LOW(?RTX_KNTXT_PTR_TABLE)
               ADD   A, B
               ADD   A, B
               MOV   DPL, A
               MOVX  A, @DPTR
               MOV   R0, A
               INC   DPTR
               MOVX  A, @DPTR
               MOV   DPH, R0
               MOV   DPL, A                     ; DPTR is ptr to the context

               INC   DPTR
               INC   DPTR                       ; DPTR-> r6 in context area
               MOV   A, R3
               MOVX  @DPTR, A                   ; Store LOW(P_MESSAGE)
               INC   DPTR                       ; DPTR -> r5 in context area
               MOV   A, R2
               MOVX  @DPTR, A                   ; Store HIGH(P_MESSAGE)
               JMP   ENQ_DONE

DRIVERS:       MOV   A, R2
               MOV   R5, A
               MOV   A, R3
               MOV   R6, A
                
ENQ_DONE:
               ; Set return 'no msg available'
               MOV   A, #1
               RET



;*----------------------------------------------------------------------*
;*      ? R T X _ M S G _ D E Q U E U E
;*----------------------------------------------------------------------*
;*      Dequeue task from message list of mailbox it waits for. Reset 
;*      msg wait bit in task state.
;*----------------------------------------------------------------------*
;*      Input:   - ?RTX_TMP0 contains task id of task to be dequeued
;*               - DPTR points to task state of task to be dequeued
;*
;*      Output:  - ?RTX_TMP0 preserved
;*               - Completion code returned in ACC: =0: dequeued
;*                                                  =1: task was not queued
;*               - If ACC=1 returned:
;*                      ACC changed, R0, R1 and DPTR preserved
;*                 else:
;*                      ACC, R0, R1 and DPTR changed
;*----------------------------------------------------------------------*

?RTX?MSG_DEQUEUE?RTXQUOP  SEGMENT CODE
                          RSEG    ?RTX?MSG_DEQUEUE?RTXQUOP

?RTX_MSG_DEQUEUE:
               ; Get task state
               MOVX  A, @DPTR
               ; Test if queued for message
               JNB   ACC.0, MSGDQ_EXIT
               
               INC   DPL                        ; If WN=1 then it is a
               MOVX  A, @DPTR                   ; semaphore wait
               DEC   DPL                        ; Restore DPTR
               JB    ACC.7, MSGDQ_EXIT
               MOVX  A, @DPTR

               ; Clear msg wait bit in task state
               ANL   A, #0FEH
               MOVX  @DPTR, A

               ; Set up ptr to ?RTX_MBX#_COUNT
               ANL   A, #070H                   ; Extract mailbox number
               RR    A                          ; from task state, shift
               ADD   A,#LOW(?RTX_MBX_CONTROL_BASE) ; Add base of mbx cont. block
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_MBX_CONTROL_BASE) ; (DPTR)->MBX#_COUNT

               GLOBAL_INT_DISABLE
               ; Decrement list count and store as new MBX#_COUNT
               MOVX  A, @DPTR                   ; (we have 2's complement,
               INC   A                          ;  therefore increment)
               MOVX  @DPTR, A

               ; Get MBX#_FIRST
               INC   DPL
               INC   DPL
               MOVX  A, @DPTR

               ; Set up DPTR as search pointer
               MOV   R0, DPL                    ; Save ptr to mbx_first
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_MBX_PAGE)
               ; Get element 1
               MOVX  A, @DPTR

               ; Increment search pointer and store as new MBX#_FIRST
               MOV   R1, DPL                    ; save search ptr
               MOV   DPL, R0                    ; Set DPL to mbx_first

               XCH   A, R1                      ; get search_ptr, save el1
               INC   A
               ANL   A, #0EFH
               MOV   DPH, #HIGH(?RTX_MBX_CONTROL_BASE)
               MOVX  @DPTR, A
               MOV   DPH, #HIGH(?RTX_MBX_PAGE)  ; set search-ptr
               MOV   DPL, A
               XCH   A, R1                      ; get el1

               ; Save element 1 temp. in R0
               MOV   R0, A
                                                ; Test if search complete
                                                ; Exit if el 1 = search taskid
MSG_WHILE_TST: CJNE  A, ?RTX_TMP0, MSG_DOWHILE
               CLR   A                          ; status = dequeued
               GLOBAL_INT_ENABLE
               RET

MSGDQ_EXIT:    MOV   A, #1                      ; status = task was not queued
               GLOBAL_INT_ENABLE
               RET

MSG_DOWHILE:   ; Search loop
               ; Get element 2
               MOVX  A, @DPTR
               ; Save el 2 in R0, get el 1
               XCH   A, R0
               ; Store el 1 moved up 1 place
               MOVX  @DPTR, A
               ; Get el 2 & make it new el 1
               ; Increment search pointer
               INC   DPL
               ANL   DPL, #0EFH
               MOV   A, R0                      ; el1=el2
               SJMP  MSG_WHILE_TST



;*----------------------------------------------------------------------*
;*      ? R T X _ N F L _ E N Q U E U E
;*----------------------------------------------------------------------*
;*      Enqueue task for non full wait. If task is enqueued, then the 
;*      wait flag in the task state is set and the mailbox number is 
;*      stored, as well.
;*----------------------------------------------------------------------*
;*      Input:   - DPTR points to MBX#_COUNT
;*               - ?RTX_RUNNING_TID contains task id of task to be enqueued
;*               - R4 contains mailbox number
;*
;*      Output:  - ?RTX_RUNNING_TID preserved
;*               - DPTR point to task state of ?RTX_RUNNING_TID (when ACC=0)
;*               - Event status returned in ACC: = 0: enqueued
;*                                               = 1: not enqueued, wait
;*                                                    list full 
;*               - DPTR and B changed
;*----------------------------------------------------------------------*

?RTX?NFL_ENQUEUE?RTXQUOP  SEGMENT CODE
                          RSEG    ?RTX?NFL_ENQUEUE?RTXQUOP

?RTX_NFL_ENQUEUE:
               ; Insert ?RTX_RUNNING_TID in non full wait list
               INC   DPL
               JNB   ?RTX_MBX_SUPPORT, NFL_FULL
               MOVX  A, @DPTR                   ; Get MBX#_TCOUNT
               JB    ACC.4, NFL_FULL            ; Check for max.16 wait. tasks
               INC   A                          ; MBX#_TCOUNT=MBX#_TCOUNT+1
               MOVX  @DPTR, A
               MOV   A, DPL
               ADD   A, #4
               MOV   DPL, A                     ; DPTR --> MBX#_TLAST
               MOVX  A, @DPTR                   ; Get MBX#_TLAST
               MOV   B, A
               ANL   A, #00FH                   ; Check if at end of FIFO
               CJNE  A, #00FH, NO_WRAP3
               MOV   A, B                       ; Rewind ptr to start of FIFO
               ANL   A, #0F0H
               JMP   INC_DONE3
NO_WRAP3:      MOV   A, B
               INC   A                          ; Not at end of FIFO, just inc
INC_DONE3:     MOVX  @DPTR, A                   ; MBX#_TLAST=MBX#_TLAST+1
               MOV   DPL, A                     ; (DPTR) --> last el. of list
               MOV   DPH, #HIGH(?RTX_MBX_PAGE)
               MOV   A, ?RTX_RUNNING_TID
               MOVX  @DPTR, A                   ; Now store ?RTX_RUNNING_TID in list
                                                ; Set up ptr to task state
               ANL   A, #01FH                   ; Reuse ?RTX_RUNNING_TID,get taskno
               RL    A
               RL    A
               ADD   A, #LOW (?RTX_STATE_TABLE) ; DPTR-->state_table[task_no]
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_STATE_TABLE)
               MOV   A, R4                      ; Get mailbox number
               SWAP  A                          ; Prepare mbx entry to
               ANL   A, #070H                   ; task state
               MOV   B, A                       ; Save temp.
               MOVX  A, @DPTR                   ; Get old task state
               ANL   A, #08FH                   ; Clear old mbx no entry
               ORL   A, B                       ; Store mailbox no
               MOVX  @DPTR, A                   ; Store task state first byte
               INC   DPL
               MOVX  A, @DPTR                   ; Get task state second byte
               ORL   A, #080H                   ; Set NT bit for non full wait
               MOVX  @DPTR, A                   ; Store task state second byte
               DEC   DPL                        ; Rewind ptr to first byte
                                                ; Set return 'task enqueued'
               CLR   A
               RET

NFL_FULL:                                       ; Set return 'wait list full'
               MOV   A, #1
               RET




;*----------------------------------------------------------------------*
;*      ? R T X _ N F L _ D E Q U E U E
;*----------------------------------------------------------------------*
;*      Dequeue task from non full wait list of mailbox it waits for. 
;*      Reset nfl wait bit in task state.
;*----------------------------------------------------------------------*
;*      Input:   - ?RTX_TMP0 contains task id of task to be dequeued
;*               - DPTR points to task state of task to be dequeued
;*
;*      Output:  - ?RTX_TMP0 preserved
;*               - Completion code returned in ACC: =0: dequeued
;*                                                  =1: task was not queued
;*               - If ACC=1 returned:
;*                      no registers changed
;*                 else:
;*                      R0, R1 and DPTR changed
;*----------------------------------------------------------------------*

?RTX?NFL_DEQUEUE?RTXQUOP  SEGMENT CODE
                          RSEG    ?RTX?NFL_DEQUEUE?RTXQUOP

?RTX_NFL_DEQUEUE:
               MOVX  A, @DPTR                   ; It is not a NFL wait
               JB    ACC.0, NFLDQ_EXIT

               ; Get second state byte
               INC   DPL
               MOVX  A, @DPTR
               ; Test if queued for non full
               JNB   ACC.7, NFLDQ_EXIT2

               ; Clear nfl wait bit in task state
               ANL   A, #07FH
               MOVX  @DPTR, A

               ; Get task state (byte 0)
               DEC   DPL
               MOVX  A, @DPTR

               ; Set up ptr to MBX#_TCOUNT
               ANL   A, #070H                   ; Extract mailbox number
               RR    A                          ; from task state, shift
               ADD   A, #LOW(?RTX_MBX_CONTROL_BASE) ; Add base of mbx contr. block
               INC   A
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_MBX_CONTROL_BASE) ; (DPTR) --> MBX#_TCOUNT

               ; Decrement list count and store as new MBX#_TCOUNT
               MOVX  A, @DPTR
               DEC   A
               MOVX  @DPTR, A

               ; Get MBX#_TFIRST
               INC   DPL
               INC   DPL
               INC   DPL
               MOVX  A, @DPTR

               ; Set up DPTR as search pointer
               MOV   R0, DPL                    ; Save ptr to mbx_first in R0
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_MBX_PAGE)
               ; Get element 1
               MOVX  A, @DPTR
               MOV   R1, A                      ; Store el1 temp. in R1

               ; Increment search pointer and store as new MBX#_TFIRST
               MOV   A, DPL
               ANL   A, #00FH
               CJNE  A, #00FH, NO_WRAP1
               MOV   A, DPL                     ; (a) rewind FIFO ptr
               ANL   A, #0F0H
               SJMP  INC_DONE1
NO_WRAP1:      MOV   A, DPL                     ; (b) incr. FIFO ptr
               INC   A
INC_DONE1:     MOV   DPH, #HIGH(?RTX_MBX_CONTROL_BASE)
               MOV   DPL, R0
               MOVX  @DPTR, A
               MOV   DPH, #HIGH(?RTX_MBX_PAGE)
               MOV   DPL, A
               ; Get element 1 back
               MOV   A, R1
               ; Test if search complete, exit if el 1 = search taskid
NFL_WHILE_TST: CJNE  A, ?RTX_TMP0, NFL_DOWHILE
               ; Return 'dequeued'
               CLR   A
               RET
                
NFLDQ_EXIT2:   DEC   DPL                        ; Rewind DPTR
               ; Return 'task not queued'
NFLDQ_EXIT:    MOV   A, #1
               RET

NFL_DOWHILE:   ; Search loop, get element 2
               MOVX  A, @DPTR
               ; Save el 2 in R1, get el 1
               XCH   A, R1
               ; Store el 1 moved up 1 place
               MOVX  @DPTR, A
               ; Increment search pointer
               MOV   A, DPL
               ANL   A, #00FH
               CJNE  A, #00FH, NO_WRAP2
               MOV   A, DPL                     ; (a) rewind FIFO ptr
               ANL   A, #0F0H
               SJMP  INC_DONE2
NO_WRAP2:      MOV   A, DPL                     ; (b) incr. FIFO ptr
               INC   A
INC_DONE2:     MOV   DPL, A
               ; Get el 2 & make it new el 1
               MOV   A, R1
               SJMP  NFL_WHILE_TST




;*----------------------------------------------------------------------*
;*      ? R T X _ S E M _ E N Q U E U E
;*----------------------------------------------------------------------*
;*      Enqueue task if not token is available.
;*      Task is not enqueued, if:
;*              a) a token is available
;*              b) no token is available, but a conditional enqueue
;*                 was specified
;*              c) task wait list is full (already 16 waiting tasks)
;*      If task is enqueued, then the wait bits in the task state are set
;*      and the semaphore number is stored, as well.
;*----------------------------------------------------------------------*
;*      Input:   - ACC: Bits 0,1,2 contain internal semaphore number
;*               - DPTR points to task state of task to be enqueued
;*               - ?RTX_RUNNING_TID contains task id of task to be enqueued
;*               - R4  = 0: conditional enqueue (do not enqueue, if
;*                          no token is available)
;*                    <> 0: unconditional enqueue (enqueue, if no
;*                          token is available)
;*
;*      Output:  - ?RTX_RUNNING_TID, R6, R4, R3, R2 preserved
;*               - Event status returned in ACC: = 0: token available
;*                                               = 1: no token available
;*                                               = 2: no token, but wait
;*                                                    list full (not queued)
;*               - ACC, B, R0, R1, R5 and DPTR changed
;*----------------------------------------------------------------------*

?RTX?SEM_ENQUEUE?RTXQUOP  SEGMENT CODE
                          RSEG    ?RTX?SEM_ENQUEUE?RTXQUOP

?RTX_SEM_ENQUEUE:
               MOV   R0, DPH
               MOV   R1, DPL                    ; Store DPTR
               ANL   A, #07H                    ; Extract internal sema number
               MOV   B, A                       ; Store sema no for later
               ADD   A, #LOW(?RTX_SEM_TOKENS)   ; Add base of token array
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_SEM_TOKENS) ; DPTR --> ?RTX_SEM_TOKENS[#]
               MOVX  A, @DPTR
               ; Check if token is available
               JNZ   TKNS_WAIT
               ; Check if conditional wait
               CJNE  R4, #0, SEM_ENQ
               ; Set return 'no tokens available'
               MOV   A, #1
               RET

TKNS_WAIT:     ; token is available (token flag is set)
               CLR   A                          ; Set return 'token available'
               MOVX  @DPTR, A                   ; Clear token flag 
               RET

SFULL:         ; Set return 'no token,list full'
               MOV   A, #2
               RET

SEM_ENQ:       ; Enqueue task in wait list
               ; Check if wait list size <> 0
               JNB   ?RTX_SEM_SUPPORT, SFULL
               MOV   A, B
               SWAP  A
               RR    A
               ADD   A, #7
               ADD   A, #LOW(?RTX_MBX_CONTROL_BASE)  ; Add base of mbx-cont-block
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_MBX_CONTROL_BASE)  ; DPTR --> SEM#_LAST
               MOVX  A, @DPTR
               MOV   R5, A                      ; R5 = old SEM#_LAST
               ANL   A, #00FH
               CJNE  A, #00FH, NSLAST                   
               MOV   A, R5                      ; (a) rewind FIFO ptr
               ANL   A, #0F0H
               SJMP  IDONE
NSLAST:        MOV   A, R5
               INC   A                          ; (b) incr. FIFO ptr
IDONE:         MOV   R5, A                      ; R5 = new SEM#_LAST
               DEC   DPL
               MOVX  A, @DPTR                   ; Get SEM#_FIRST
               CLR   C
               SUBB  A, R5
               JZ    SFULL                      ; Test if SEM#_FIRST=SEM#_LAST
               INC   DPL
               MOVX  A, @DPTR                   ; Get back old SEM#_LAST
               XCH   A, R5                      ; ACC=new/R5=old SEM#_LAST
               MOVX  @DPTR, A                   ; Store new SEM#_LAST
               MOV   DPL, R5                    ; Use old SEM#_LAST
               MOV   DPH, #HIGH(?RTX_SEM_PAGE)  
               MOV   A, ?RTX_RUNNING_TID
               MOVX  @DPTR, A                   ; ^old SEM#_LAST = task id

               ; Set wait flag and store internal semaphore no
               MOV   A, B
               SWAP  A
               SETB  ACC.0
               MOV   B, A                       ; B = 0###0001 (#=sem no)
               MOV   DPH, R0                    ; Get back ptr to task state
               MOV   DPL, R1
               MOVX  A, @DPTR                   
               ANL   A, #08FH                   ; Clear sema number field
               ORL   A, B                       ; Store sem no, set WM bit
               MOVX  @DPTR, A                   ; state0 |= 0###0001 

               INC   DPL
               MOVX  A, @DPTR
               ORL   A, #080H                   ; Set WN bit
               MOVX  @DPTR, A                   ; state1 |= 10000000

               ; Set return 'no token available'
               MOV   A, #1
               RET



;*----------------------------------------------------------------------*
;*      ? R T X _ S E M _ D E Q U E U E
;*----------------------------------------------------------------------*
;*      Dequeue task from wait list of semaphore it waits for. Reset 
;*      semaphore wait bits in task state.
;*----------------------------------------------------------------------*
;*      Input:   - ?RTX_TMP0 contains task id of task to be dequeued
;*               - DPTR points to task state of task to be dequeued
;*
;*      Output:  - ?RTX_TMP0 preserved
;*               - ACC, R0, R1 and DPTR changed
;*----------------------------------------------------------------------*

?RTX?SEM_DEQUEUE?RTXQUOP  SEGMENT CODE
                          RSEG    ?RTX?SEM_DEQUEUE?RTXQUOP

?RTX_SEM_DEQUEUE:
               ; Get task state
               MOVX  A, @DPTR
               ; Test if queued for message
               JNB   ACC.0, SEMDQ_EXIT          ; If WM = 0: no wait for sem.
               
               INC   DPL                        ; If WN=1 then it is a
               MOVX  A, @DPTR                   ; semaphore wait
               JNB   ACC.7, SEMDQ_EXIT
               ; Clear WN wait bit in task state
               ANL   A, #07FH
               MOVX  @DPTR, A
               DEC   DPL
               MOVX  A, @DPTR

               ; Clear WM wait bit in task state
               ANL   A, #0FEH
               MOVX  @DPTR, A

               ; Set up ptr to SEM#_FIRST
               ANL   A, #070H                   ; Extract semaphore number
               RR    A                          ; from task state, shift
               ADD   A,#LOW(?RTX_MBX_CONTROL_BASE) ; Add base of mbx cont. block
               ADD   A, #6
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_MBX_CONTROL_BASE) ; (DPTR)->SEM#_FIRST

               ; Get SEM#_FIRST
               MOVX  A, @DPTR

               ; Set up DPTR as search pointer
               MOV   R0, DPL                    ; R0 = ptr to SEM#_FIRST
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_SEM_PAGE)
               ; Get element 1
               MOVX  A, @DPTR
               MOV   R1, A                      ; R1 = el1

               ; Increment search pointer and store as new SEM#_FIRST
               MOV   A, DPL
               ANL   A, #00FH
               CJNE  A, #00FH, NO_WRAP6
               MOV   A, DPL                     ; (a) rewind FIFO ptr
               ANL   A, #0F0H
               SJMP  INC_DONE6
NO_WRAP6:      MOV   A, DPL                     ; (b) incr. FIFO ptr
               INC   A
INC_DONE6:     MOV   DPH, #HIGH(?RTX_MBX_CONTROL_BASE)
               MOV   DPL, R0
               MOVX  @DPTR, A                   ; Store new SEM#_FIRST
               MOV   DPH, #HIGH(?RTX_SEM_PAGE)
               MOV   DPL, A
               ; Get element 1 back
               MOV   A, R1
               ; Test if search complete, exit if el 1 = search taskid
SEM_WHILE_TST: CJNE  A, ?RTX_TMP0, SEM_DOWHILE
               ; All done
SEMDQ_EXIT:    RET

SEM_DOWHILE:   ; Search loop, get element 2
               MOVX  A, @DPTR
               ; Save el 2 in R1, get el 1
               XCH   A, R1
               ; Store el 1 moved up 1 place
               MOVX  @DPTR, A
               ; Increment search pointer
               MOV   A, DPL
               ANL   A, #00FH
               CJNE  A, #00FH, NO_WRAP7
               MOV   A, DPL                     ; (a) rewind FIFO ptr
               ANL   A, #0F0H
               SJMP  INC_DONE7
NO_WRAP7:      MOV   A, DPL                     ; (b) incr. FIFO ptr
               INC   A
INC_DONE7:     MOV   DPL, A
               ; Get el2 & make it new el1
               MOV   A, R1
               SJMP  SEM_WHILE_TST



;*----------------------------------------------------------------------*
;*      ? R T X _ T M O _ E N Q U E U E
;*----------------------------------------------------------------------*
;*      Enqueue task for timeout wait. Set tmo wait bit in task state.
;*----------------------------------------------------------------------*
;*      Input:   - DPTR points to task state of task to be enqueued
;*               - ?RTX_RUNNING_TID contains task id of task to be enqueued
;*               - R2 contains timeout value
;*
;*      Output:  - ?RTX_RUNNING_TID preserved
;*               - Event status returned in ACC: = 0: zero timeout
;*                                               = 1: enqueued
;*                                               = 2: not enqueued
;*               - ACC and DPTR changed
;*----------------------------------------------------------------------*

?RTX?TMO_ENQUEUE?RTXQUOP  SEGMENT CODE
                          RSEG    ?RTX?TMO_ENQUEUE?RTXQUOP

?RTX_TMO_ENQUEUE:
               ; Check if enqueue required
               MOV   A, R2
               JZ    NOTMO
               CJNE  A, #0FFH, TMO_ENQ
               JMP   NOTMO_ENQ

               ; Set tmo wait bit in task state
TMO_ENQ:       MOVX  A, @DPTR
               ORL   A, #008H
               MOVX  @DPTR, A
               ; Enqueue task

               ; Calculate timeout count and store in delay list
               MOV   A, ?RTX_RUNNING_TID
               ANL   A, #01FH                   ; Get task nr.
               ADD   A, #LOW(?RTX_DELAY_LIST_CLK)
               MOV   DPL, A
               MOV   A, ?RTX_SYSCLK_COUNT
               ADD   A, R2
               ADDC  A, #0                      ; overflow correction
               CJNE  A, #0FFH, NO_WRAP4         ; FFH is reserved
               MOV   A, #0
NO_WRAP4:      MOV   DPH, #HIGH(?RTX_DELAY_LIST_CLK)
               MOVX  @DPTR, A

               ; Store task id in delay list
               MOV   A, DPL
               ADD   A, #TASK_COUNT
               MOV   DPL, A
               MOV   A, ?RTX_RUNNING_TID
               MOVX  @DPTR, A

               ; Set return 'enqueued'
               MOV   A, #1
               RET

NOTMO:         ; Set return 'zero timeout'
               CLR   A
               RET

NOTMO_ENQ:     ; Set return 'not enqueued'
               MOV   A, #2
               RET



;*----------------------------------------------------------------------*
;*      ? R T X _ I V L _ E N Q U E U E
;*----------------------------------------------------------------------*
;*      Enqueue task for interval wait. Set tmo wait bit in task state.
;*----------------------------------------------------------------------*
;*      Input:   - DPTR points to task state of task to be enqueued
;*               - ?RTX_RUNNING_TID contains task id of task to be enqueued
;*               - R2 contains interval value
;*
;*      Output:  - ?RTX_RUNNING_TID preserved
;*               - Event status returned in ACC: = 0: interval event (al-
;*                                                    ready passed)
;*                                               = 1: enqueued
;*               - If ACC=1 returned:
;*                      B, R5 and DPTR changed
;*                 else:
;*                      B, R5 changed, DPTR preserved
;*----------------------------------------------------------------------*

?RTX?IVL_ENQUEUE?RTXQUOP  SEGMENT CODE
                          RSEG    ?RTX?IVL_ENQUEUE?RTXQUOP

?RTX_IVL_ENQUEUE:
               ; Set tmo wait bit in task state
               MOVX  A, @DPTR
               ORL   A, #008H
               MOVX  @DPTR, A
               MOV   R5, DPL                    ; Store temp. in R5
               ; Calculate timeout count
               MOV   A, ?RTX_RUNNING_TID
               ANL   A, #01FH                   ; Get task nr.
               ADD   A, #LOW(?RTX_INTERVAL_LIST)
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_INTERVAL_LIST)
               MOVX  A, @DPTR
               CJNE  A, #0FFH, NOTFIRST
               MOV   A, ?RTX_SYSCLK_COUNT       ; Take actual time if first wait
NOTFIRST:      
               MOV   B, A                       ; B: last wake-up time
               MOV   A, ?RTX_SYSCLK_COUNT
               CLR   C
               SUBB  A, B                       ; i = act_time-last_wakeup_time
               CLR   C
               SUBB  A, R2                      ; i-interval >= 0: interval
               JNC   IVLEVENT                   ; already passed
               MOV   A, B
               ADD   A, R2
               ADDC  A, #0                      ; overflow correction
               CJNE  A, #0FFH, NO_WRAP8         ; FFH is reserved
               MOV   A, #0
NO_WRAP8:      CJNE  A, ?RTX_SYSCLK_COUNT, NIEV ; Check again for "passed"
                                                ; (including correction)
IVLEVENT:      ; Store actual time as new wake-up time  
               MOV   A, ?RTX_RUNNING_TID
               ANL   A, #01FH 
               ADD   A, #LOW(?RTX_INTERVAL_LIST)
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_INTERVAL_LIST) 
               MOV   A, ?RTX_SYSCLK_COUNT
               MOVX  @DPTR, A
               ; Reset TMO wait bit
               MOV   DPL, R5
               MOV   DPH, #HIGH(?RTX_STATE_TABLE)
               MOVX  A, @DPTR
               ANL   A, #0F7H
               MOVX  @DPTR, A
               ; status = interval already passed
               CLR   A
               RET

NIEV:          XCH   A, R2                      ; Store temp. in R2
               ; Store timeout count in delay list
               MOV   A, DPL
               CLR   C
               SUBB  A, #(2*TASK_COUNT)
               MOV   DPL, A
               XCH   A, R2
               MOVX  @DPTR, A

               ; Store task id in delay list
               MOV   A, DPL
               ADD   A, #TASK_COUNT
               MOV   DPL, A
               MOV   A, ?RTX_RUNNING_TID
               MOVX  @DPTR, A
               ; Status = enqueued for interval
               MOV   A, #1
               RET


;*----------------------------------------------------------------------*
;*      ? R T X _ T M O _ D E Q U E U E
;*----------------------------------------------------------------------*
;*      Dequeue task from delay list. Reset tmo wait bit in task state.
;*----------------------------------------------------------------------*
;*      Input:   - ?RTX_TMP0 contains task id of task to be dequeued
;*               - DPTR points to task state of task to be dequeued
;*
;*      Output:  - ?RTX_TMP0 and DPTR preserved
;*               - ACC and R0 changed
;*----------------------------------------------------------------------*

?RTX?TMO_DEQUEUE?RTXQUOP  SEGMENT CODE
                          RSEG    ?RTX?TMO_DEQUEUE?RTXQUOP

?RTX_TMO_DEQUEUE:                               ; Get task state
               MOVX  A, @DPTR
                                                ; Test if queued
               JNB   ACC.3, TMODQ_EXIT
                                                ; Clear tmo wait bit
                                                ; in task state
               ANL   A, #0F7H
               MOVX  @DPTR, A

               ; preserve DPTR
               MOV   R0, DPL
               ; Set up pointer to delay_list_clk[task_no]
               MOV   A, ?RTX_TMP0               ; Get task id
               ANL   A, #01FH                   ; Extract task number
               ADD   A, #LOW(?RTX_DELAY_LIST_CLK) ; Add base address of table
               MOV   DPL, A
               MOV   DPH, #HIGH(?RTX_DELAY_LIST_CLK)

               ; Store 0FFH in delay list as a delete mark
               MOV   A, #0FFH
               MOVX  @DPTR, A
               ; restore DPTR
               MOV   DPH, #HIGH(?RTX_STATE_TABLE)
               MOV   DPL, R0
               ; Exit
TMODQ_EXIT:    RET



;*----------------------------------------------------------------------*
;*      ? R T X _ I N T _ E N Q U E U E
;*----------------------------------------------------------------------*
;*      Enqueue task in interrupt wait list. Sets wait bit in task state.
;*----------------------------------------------------------------------*
;*      Input:   - DPTR points to task state of task to be enqueued
;*               - ?RTX_RUNNING_TID contains task id of task to be enqueued
;*               - R4  = 0: do not enqueue
;*                    <> 0: enqueue
;*
;*      Output:  - DPTR and R5 preserved
;*               - ACC and R0 changed
;*----------------------------------------------------------------------*

?RTX?INT_ENQUEUE?RTXQUOP  SEGMENT CODE
                          RSEG    ?RTX?INT_ENQUEUE?RTXQUOP

?RTX_INT_ENQUEUE:
               ; Check if conditional enq.
               CJNE  R4, #0, INT_ENQ
               RET

INT_ENQ:       ; Get task state
               MOVX  A, @DPTR
               ; Set interrupt wait bit in task state
               ORL   A, #002H
               MOVX  @DPTR, A

               ; Use DPTR as ptr to int masks Activate interrupts
               MOV   R0, DPL                    ; preserve DPL
               INC   DPL
               MOVX  A, @DPTR                   ; Get int. mask 0

               GLOBAL_INT_DISABLE               ; Disable ISR Int's while
                                                ; updating the INT-Masks
               ORL   ?RTX_ND_IE, A              ; Enable int. in ND mask
               JNB   ?RTX_RUNNING_TID.4, DONE0
               ORL   ?RTX_D_IE,A                ; Enable int. in D mask
DONE0:
               JNB   ?RTX_ENA_INT_REG1, DONE2
               INC   DPL
               MOVX  A, @DPTR                   ; Get int. mask 1
               ORL   ?RTX_ND_IE1, A             ; Enable int. in ND mask
               JNB   ?RTX_RUNNING_TID.4, DONE1
               ORL   ?RTX_D_IE1, A              ; Enable int. in D mask
DONE1:
               JNB   ?RTX_ENA_INT_REG2, DONE2
               INC   DPL
               MOVX  A, @DPTR                   ; Get int. mask 2
               ORL   ?RTX_ND_IE2, A             ; Enable int. in ND mask
               JNB   ?RTX_RUNNING_TID.4, DONE2
               ORL   ?RTX_D_IE2, A              ; Enable int. in D mask
DONE2:
               GLOBAL_INT_ENABLE
               MOV   DPL, R0                    ; restore DPL
               RET



;*----------------------------------------------------------------------*
;*      ? R T X _ I N T S I G _ D E Q U E U E
;*----------------------------------------------------------------------*
;*      Dequeue task from interrupt and signal wait lists. Reset wait 
;*      bits in task state.
;*----------------------------------------------------------------------*
;*      Input:   - ?RTX_TMP0 contains task id of task to be dequeued
;*               - DPTR points to task state of task to be dequeued
;*
;*      Output:  - DPTR and ?RTX_TMP0 preserved
;*               - ACC and R0 changed
;*----------------------------------------------------------------------*

?RTX?INTSIG_DEQUEUE?RTXQUOP  SEGMENT CODE
                             RSEG    ?RTX?INTSIG_DEQUEUE?RTXQUOP

?RTX_INTSIG_DEQUEUE:                            ; Get task state
               MOVX  A, @DPTR
                                                ; Test if queued for interrupt
               JNB   ACC.1, NOINT_QU
                                                ; Clear int and sig wait bits
                                                ; in task state
               ANL   A, #0F9H
               MOVX  @DPTR, A

               MOV   R0, DPL                    ; Save DPTR
               ; Dequeue from interrupt lists
               INC   DPTR
               MOVX  A, @DPTR                   ; ACC : int. bit mask
               ANL   A, #07FH                   ; Bit 7 must be 0 !
               CPL   A

               GLOBAL_INT_DISABLE               ; Disable ISR Int's while
                                                ; updating the INT-Masks
               ANL   ?RTX_ND_IE, A              ; Disable int. in ND mask
               ANL   ?RTX_D_IE, A               ; Disable int. in D mask

               JNB   ?RTX_ENA_INT_REG1, DEQUEU_DONE
               INC   DPTR                       ; R1 : Pointer to int. mask 1
               MOVX  A, @DPTR                   ; ACC : int. bit mask 1
               CPL   A
               ANL   ?RTX_ND_IE1, A             ; Disable int. in ND mask
               ANL   ?RTX_D_IE1, A              ; Disable int. in D mask

               JNB   ?RTX_ENA_INT_REG2, DEQUEU_DONE
               INC   DPTR                       ; R1 : Pointer to int. mask 2
               MOVX  A, @DPTR                   ; ACC : int. bit mask 2
               CPL   A
               ANL   ?RTX_ND_IE2, A             ; Disable int. in ND mask
               ANL   ?RTX_D_IE2, A              ; Disable int. in D mask
DEQUEU_DONE:
               GLOBAL_INT_ENABLE
               ; restore DPTR
               MOV   DPH, #HIGH(?RTX_STATE_TABLE)
               MOV   DPL, R0
               RET

NOINT_QU:      ; Clear sig wait bit in task state
               ANL   A, #0FBH
               MOVX  @DPTR, A
               RET

;						       
;   END OF MODULE				       
;
	END
